/***************************************************************************************************************************
* position_controller.cpp
*
* Author: SFL
*
* Update Time: 2020.8.8
*
* Introduction:  PX4 Position Controller 
*         1. 从应用层节点订阅/px4_command/control_command话题（ControlCommand.msg），接收来自上层的控制指令。
*         2. 从command_from_mavros.h读取无人机的状态信息（DroneState.msg）。
*         3. 不使用任何算法，直接将参考位置信息输出，利用飞控中自己的位置环算法获得飞机的状态更新
*         4. 通过command_to_mavros.h将计算出来的控制指令发送至飞控（通过mavros包）(mavros package will send the message to PX4 as Mavlink msg)
*         5. PX4 firmware will recieve the Mavlink msg by mavlink_receiver.cpp in mavlink module.
*         6. 发送相关信息至地面站节点(/px4_command/topic_for_log)，供监控使用。
***************************************************************************************************************************/

#include <ros/ros.h>
#include <Eigen/Eigen>

#include <state_from_mavros.h>
#include <command_to_mavros.h>

#include <px4_command_utils.h>
#include <path_planning_utils.h>
#include <height_controller_cascade_PID.h>
#include <LR_controller_cascade_PID.h>

#include <circle_trajectory.h>
#include <eight_trajectory.h>

#include <wrzf_pkg/ControlCommand.h>
#include <wrzf_pkg/DroneState.h>
#include <wrzf_pkg/Topic_for_log.h>
#include "fsd_common_msgs/img_pro_info.h"
#include "TFLuna_i2c/RadarAvoidance.h"

using namespace std;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>变量声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
wrzf_pkg::ControlCommand Command_Now;                      //无人机当前执行命令
wrzf_pkg::ControlCommand Command_Last;                     //无人机上一条执行命令
wrzf_pkg::ControlCommand Command_to_gs;                    //发送至地面站的指令
wrzf_pkg::DroneState _DroneState;                          //无人机状态量
float d_pos_body[2];        //the desired xy position in Body Frame
float d_pos_enu[2];         //the desired xy position in enu Frame (The origin point is the drone)
int CAM_CENTER_X = 0;                                        //摄像头视野中心坐标
int CAM_CENTER_Z = 0;
wrzf_pkg::Topic_for_log _Topic_for_log;                  //用于日志记录的topic
fsd_common_msgs::img_pro_info camera_data;                         //摄像头捕捉到的框的信息
fsd_common_msgs::img_pro_info camera_data_last;  

float cam_timeout = 0.0;                                      //判断摄像头是否丢失
int kill_cam = 0;

TFLuna_i2c::RadarAvoidance radar_avoidance_data;              //雷达避障数据（有判断是否有障碍物flag以及避障方向）
Eigen::Vector3d pos_sp;
Eigen::Quaterniond quaternion_sp;
Eigen::Vector3d rpy_sp;
float thrust_sp;
float forward_pitch;
float cur_time;
float Takeoff_height;                                       //起飞高度
float Disarm_height;                                        //自动上锁高度
// float Use_accel;                                            // 1 for use the accel command
int Flag_printf;
double ahead_distance;//实时目标点向前的距离
double k; //速度的增益

//Geigraphical fence 地理围栏
Eigen::Vector2f geo_fence_x;
Eigen::Vector2f geo_fence_y;
Eigen::Vector2f geo_fence_z;
//起飞的位置
Eigen::Vector3d Takeoff_position = Eigen::Vector3d(0.0,0.0,0.0);

geometry_msgs::Point end_point, waypoint;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>函数声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int check_failsafe();
void printf_param();
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void Command_cb(const wrzf_pkg::ControlCommand::ConstPtr& msg)
{
    Command_Now = *msg;
    
    // 无人机一旦接受到Land指令，则会屏蔽其他指令
    // if(Command_Last.Mode == command_to_mavros::Land)
    // {
    //     Command_Now.Mode = command_to_mavros::Land;
    // }
}

void drone_state_cb(const wrzf_pkg::DroneState::ConstPtr& msg)
{
    _DroneState = *msg;

    _DroneState.time_from_start = cur_time;

        // Check for geo fence: If drone is out of the geo fence, it will land now.
    if(check_failsafe() == 1)
    {
        Command_Now.Mode = wrzf_pkg::ControlCommand::Land;
    }
}

void cam_cb(const fsd_common_msgs::img_pro_info::ConstPtr& msg)
{
    camera_data = *msg;
}

void radar_avoidance_cb(const TFLuna_i2c::RadarAvoidance::ConstPtr& msg)
{
    radar_avoidance_data = *msg;
}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "position_controller");
    ros::NodeHandle nh("~");

    //【订阅】指令
    // 本话题来自根据需求自定义的上层模块，比如track_land.cpp 比如move.cpp
    ros::Subscriber Command_sub = nh.subscribe<wrzf_pkg::ControlCommand>("/px4_command/control_command", 1000, Command_cb);
    
    //【订阅】无人机当前状态
    // 本话题来自根据需求自定px4_pos_estimator.cpp
    ros::Subscriber drone_state_sub = nh.subscribe<wrzf_pkg::DroneState>("/px4_command/drone_state", 10, drone_state_cb);
    
    //【订阅】摄像头获取到的框在像素坐标系下的位置
    ros::Subscriber cam_sub = nh.subscribe<fsd_common_msgs::img_pro_info>("/img_pro_info", 1, cam_cb);

    //【订阅】雷达避障动作数据
    ros::Subscriber radar_avoidance_sub = nh.subscribe<TFLuna_i2c::RadarAvoidance>("/radar_avoidance_data", 1, radar_avoidance_cb);

    // 发布log消息至ground_station.cpp
    ros::Publisher log_pub = nh.advertise<wrzf_pkg::Topic_for_log>("/px4_command/topic_for_log", 10);

    // 参数读取
    nh.param<float>("Takeoff_height", Takeoff_height, 1.0);
    nh.param<float>("Disarm_height", Disarm_height, 0.15);
    // nh.param<float>("Use_accel", Use_accel, 0.0);
    nh.param<int>("Flag_printf", Flag_printf, 0.0);
    
    nh.param<float>("geo_fence/x_min", geo_fence_x[0], -10.0);
    nh.param<float>("geo_fence/x_max", geo_fence_x[1], 10.0);
    nh.param<float>("geo_fence/y_min", geo_fence_y[0], -10.0);
    nh.param<float>("geo_fence/y_max", geo_fence_y[1], 10.0);
    nh.param<float>("geo_fence/z_min", geo_fence_z[0], -10.0);
    nh.param<float>("geo_fence/z_max", geo_fence_z[1], 10.0);

    nh.param<float>("forward_pitch", forward_pitch, 0.2);
    nh.param<double>("ahead_distance", ahead_distance, 1.0);
    nh.param<double>("k", k, 0.1);

    // 位置控制选取为50Hz，主要取决于位置状态的更新频率
    ros::Rate rate(50);
    // 用于与mavros通讯的类，通过mavros发送控制指令至飞控【本程序->mavros->飞控】
    command_to_mavros _command_to_mavros;
    height_controller_cascade_PID height_controller_cascade_pid;
    LR_controller_cascade_PID lr_controller_cascade_pid;

    // 圆形轨迹追踪类
    Circle_Trajectory _Circle_Trajectory;
    Eight_Trajectory _Eight_Trajectory;
    float time_trajectory = 0.0;

    //*************************************路径规划****************************************
    vector<double> x = {0.0, 2.0, 3.0, 4.0, 5.0, 4.5, 3.5, 2.0};//规划路径的关键航点x
    vector<double> y = {0.0, 0.5, 0.0, -0.5,0.0, 1.5, 2.5, 2.0};//规划路径的关键航点y
    // vector<double> x = {0.0, 153.003, 203.003, 253.003, 353.003, 453.003, 553.003, 653.003, 703.003, 753.003, 752.999, 702.999, 652.999, 552.999, 452.999, 352.999, 252.999, 202.999, 152.999};//规划路径的关键航点x
    // vector<double> y = {0.0, 50.9527, 50.9527, 50.9527, 50.9527, 50.9527, 50.9527, 50.9527, 50.9527, 50.9527, 111.228, 111.228, 111.228, 111.228, 111.228, 111.228, 111.228, 111.228, 111.228};//规划路径的关键航点y
    // vector<double> x = {0.0, 153.003/30, 203.003/30, 253.003/30, 353.003/30, 453.003/30, 553.003/30, 653.003/30, 703.003/30, 753.003/30, 752.999/30, 702.999/30, 652.999/30, 552.999/30, 452.999/30, 352.999/30, 252.999/30, 202.999/30, 152.999/30};//规划路径的关键航点x
    // vector<double> y = {0.0, 50.9527/10, 50.9527/10, 50.9527/10, 50.9527/10, 50.9527/10, 50.9527/10, 50.9527/10, 50.9527/10, 50.9527/10, 111.228/10, 111.228/10, 111.228/10, 111.228/10, 111.228/10, 111.228/10, 111.228/10, 111.228/10, 111.228/10};//规划路径的关键航点y
    
    vector<double> tx;  //x坐标
    vector<double> ty;  //y坐标
    vector<double> tyaw;//偏航
    vector<double> tc;  //曲率
    int target_index;          //目标索引

    Spline2D csp = generate_target_course(x, y, tx, ty, tyaw, tc);
    //************************************************************************************
    printf_param();

    int check_flag;
    // 这一步是为了程序运行前检查一下参数是否正确
    // 输入1,继续，其他，退出程序
    cout << "Please check the parameter and setting，enter 1 to continue， else for quit: "<<endl;
    cin >> check_flag;

    if(check_flag != 1)
    {
        return -1;
    }

    // 先读取一些飞控的数据
    for(int i=0;i<50;i++)
    {
        ros::spinOnce();
        rate.sleep();
    }

    // Set the takeoff position
    Takeoff_position[0] = _DroneState.position[0];
    Takeoff_position[1] = _DroneState.position[1];
    Takeoff_position[2] = _DroneState.position[2];

    // 初始化命令-
    // 默认设置：Idle模式 电机怠速旋转 等待来自上层的控制指令
    Command_Now.Mode = wrzf_pkg::ControlCommand::Idle;
    Command_Now.Command_ID = 0;
    Command_Now.Reference_State.Sub_mode  = command_to_mavros::XYZ_POS;
    Command_Now.Reference_State.position_ref[0] = 0;
    Command_Now.Reference_State.position_ref[1] = 0;
    Command_Now.Reference_State.position_ref[2] = 0;
    Command_Now.Reference_State.velocity_ref[0] = 0;
    Command_Now.Reference_State.velocity_ref[1] = 0;
    Command_Now.Reference_State.velocity_ref[2] = 0;
    Command_Now.Reference_State.acceleration_ref[0] = 0;
    Command_Now.Reference_State.acceleration_ref[1] = 0;
    Command_Now.Reference_State.acceleration_ref[2] = 0;
    Command_Now.Reference_State.yaw_ref = 0;

    // 记录启控时间
    ros::Time begin_time = ros::Time::now();
    float last_time = px4_command_utils::get_time_in_sec(begin_time);
    float dt = 0;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主  循  环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    while (ros::ok())
    {
        // 当前时间
        cur_time = px4_command_utils::get_time_in_sec(begin_time);
        dt = cur_time  - last_time;
        dt = constrain_function2(dt, 0.01, 0.03);
        last_time = cur_time;

        //执行回调函数
        ros::spinOnce();
        
        if (camera_data.header.seq == camera_data_last.header.seq)
        {
            cam_timeout += dt;
        }
        else
        {
            cam_timeout = 0.0;
            kill_cam = 0;
        }
        if (cam_timeout > 1.0 && kill_cam==0)
        {
            system("rosnode kill /usb_cam");
            kill_cam = kill_cam + 1;
        }
        if (cam_timeout > 3.0)
        {
            if (_DroneState.gps_status >= 0)
            {
                _command_to_mavros.returnhome();
            }
            else
            {
                thrust_sp = 0.6;
                rpy_sp[0] = 0.0;
                rpy_sp[1] = 0.0;
                rpy_sp[2] = 0.0;
                quaternion_sp = quaternion_from_rpy(rpy_sp);
                _command_to_mavros.send_attitude_setpoint(quaternion_sp,thrust_sp);  
            }
        }
        
        camera_data_last = camera_data;
        //状态机开启
        switch (Command_Now.Mode)
        {
            // 【Idle】 怠速旋转，此时可以切入offboard模式，但不会起飞。
            case wrzf_pkg::ControlCommand::Idle:
                Command_to_gs.Mode = Command_Now.Mode;
                Command_to_gs.Command_ID = Command_Now.Command_ID;
                Command_to_gs.Reference_State.position_ref[0] = Takeoff_position[0];
                Command_to_gs.Reference_State.position_ref[1] = Takeoff_position[1];
                Command_to_gs.Reference_State.position_ref[2] = Takeoff_position[2];
                Command_to_gs.Reference_State.yaw_ref = _DroneState.attitude[2];
                _command_to_mavros.idle();
                break;
            // 【Takeoff】 从摆放初始位置原地起飞至指定高度，偏航角也保持当前角度
            case wrzf_pkg::ControlCommand::Takeoff:
                Command_to_gs.Mode = Command_Now.Mode;
                Command_to_gs.Command_ID = Command_Now.Command_ID;
                Command_to_gs.Reference_State.Sub_mode  = command_to_mavros::XYZ_POS;
                Command_to_gs.Reference_State.position_ref[0] = Takeoff_position[0];
                Command_to_gs.Reference_State.position_ref[1] = Takeoff_position[1];
                Command_to_gs.Reference_State.position_ref[2] = Takeoff_position[2] + Takeoff_height;
                Command_to_gs.Reference_State.velocity_ref[0] = 0;
                Command_to_gs.Reference_State.velocity_ref[1] = 0;
                Command_to_gs.Reference_State.velocity_ref[2] = 0;
                Command_to_gs.Reference_State.acceleration_ref[0] = 0;
                Command_to_gs.Reference_State.acceleration_ref[1] = 0;
                Command_to_gs.Reference_State.acceleration_ref[2] = 0;
                Command_to_gs.Reference_State.yaw_ref = _DroneState.attitude[2]; //rad

                pos_sp[0] = Command_to_gs.Reference_State.position_ref[0];
                pos_sp[1] = Command_to_gs.Reference_State.position_ref[1];
                pos_sp[2] = Command_to_gs.Reference_State.position_ref[2];

                _command_to_mavros.send_pos_setpoint(pos_sp, Command_to_gs.Reference_State.yaw_ref);
                
                // 测试没有GPS时能offboard控制
                // thrust_sp = 0.6 + 0.0006 * (camera_data.y_pos - CAM_CENTER_Z);

                // rpy_sp[0] = + 0.001 * (camera_data.x_pos - CAM_CENTER_X);;
                // rpy_sp[1] = 0.0;
                // rpy_sp[2] = _DroneState.attitude[2] - 0.001 * camera_data.height_difference;//添加yaw轴补偿
                
                // quaternion_sp = quaternion_from_rpy(rpy_sp);
                
                // _command_to_mavros.send_attitude_setpoint(quaternion_sp,thrust_sp);
                break;
            // 【Move_ENU】 ENU系移动。
            case wrzf_pkg::ControlCommand::Move_ENU:
                Command_to_gs = Command_Now;
                pos_sp[0] = Command_to_gs.Reference_State.position_ref[0];
                pos_sp[1] = Command_to_gs.Reference_State.position_ref[1];
                pos_sp[2] = Command_to_gs.Reference_State.position_ref[2];

                _command_to_mavros.send_pos_setpoint(pos_sp, Command_to_gs.Reference_State.yaw_ref);
                break;
            // 【Move_Body】 机体系移动。速度控制未完成
            case wrzf_pkg::ControlCommand::Move_Body:
                Command_to_gs.Mode = Command_Now.Mode;
                Command_to_gs.Command_ID = Command_Now.Command_ID;
                //只有在comid增加时才会进入解算 ： 机体系 至 惯性系
                if( Command_Now.Command_ID  >  Command_Last.Command_ID )
                {
                    //xy velocity mode
                    if( Command_Now.Reference_State.Sub_mode  & 0b10 )
                    {
                        // float d_vel_body[2] = {Command_Now.Reference_State.velocity_ref[0], Command_Now.Reference_State.velocity_ref[1]};         //the desired xy velocity in Body Frame
                        // float d_vel_enu[2];                                                           //the desired xy velocity in NED Frame

                        // //根据无人机当前偏航角进行坐标系转换
                        // px4_command_utils::rotation_yaw(_DroneState.attitude[2], d_vel_body, d_vel_enu);
                        // Command_to_gs.Reference_State.position_ref[0] = 0;
                        // Command_to_gs.Reference_State.position_ref[1] = 0;
                        // Command_to_gs.Reference_State.velocity_ref[0] = d_vel_enu[0];
                        // Command_to_gs.Reference_State.velocity_ref[1] = d_vel_enu[1];
                        cout << "速度控制还未写好，敬请期待" << endl;
                        break;
                    }
                    //xy position mode
                    else
                    {
                        float d_pos_body[2] = {Command_Now.Reference_State.position_ref[0], Command_Now.Reference_State.position_ref[1]};         //the desired xy position in Body Frame
                        float d_pos_enu[2];                                                           //the desired xy position in enu Frame (The origin point is the drone)
                        px4_command_utils::rotation_yaw(_DroneState.attitude[2], d_pos_body, d_pos_enu);

                        Command_to_gs.Reference_State.position_ref[0] = _DroneState.position[0] + d_pos_enu[0];
                        Command_to_gs.Reference_State.position_ref[1] = _DroneState.position[1] + d_pos_enu[1];
                        Command_to_gs.Reference_State.velocity_ref[0] = 0;
                        Command_to_gs.Reference_State.velocity_ref[1] = 0;
                    }

                    //z velocity mode
                    if( Command_Now.Reference_State.Sub_mode  & 0b01 )
                    {
                        // Command_to_gs.Reference_State.position_ref[2] = 0;
                        // Command_to_gs.Reference_State.velocity_ref[2] = Command_Now.Reference_State.velocity_ref[2];
                        cout << "速度控制还未写好，敬请期待" << endl;
                        break;
                    }
                    //z posiiton mode
                    {
                        Command_to_gs.Reference_State.position_ref[2] = _DroneState.position[2] + Command_Now.Reference_State.position_ref[2];
                        Command_to_gs.Reference_State.velocity_ref[2] = 0; 
                    }

                    Command_to_gs.Reference_State.yaw_ref = _DroneState.attitude[2] + Command_Now.Reference_State.yaw_ref;

                    float d_acc_body[2] = {Command_Now.Reference_State.acceleration_ref[0], Command_Now.Reference_State.acceleration_ref[1]};       
                    float d_acc_enu[2]; 

                    px4_command_utils::rotation_yaw(_DroneState.attitude[2], d_acc_body, d_acc_enu);
                    Command_to_gs.Reference_State.acceleration_ref[0] = d_acc_enu[0];
                    Command_to_gs.Reference_State.acceleration_ref[1] = d_acc_enu[1];
                    Command_to_gs.Reference_State.acceleration_ref[2] = Command_Now.Reference_State.acceleration_ref[2];

                }
                pos_sp[0] = Command_to_gs.Reference_State.position_ref[0];
                pos_sp[1] = Command_to_gs.Reference_State.position_ref[1];
                pos_sp[2] = Command_to_gs.Reference_State.position_ref[2];

                _command_to_mavros.send_pos_setpoint(pos_sp, Command_to_gs.Reference_State.yaw_ref);
                break;
            // 【Hold】 悬停。当前位置悬停
            case wrzf_pkg::ControlCommand::Hold:
                Command_to_gs.Mode = Command_Now.Mode;
                Command_to_gs.Command_ID = Command_Now.Command_ID;
                if (Command_Last.Mode != wrzf_pkg::ControlCommand::Hold)
                {
                    Command_to_gs.Reference_State.Sub_mode  = command_to_mavros::XYZ_POS;
                    Command_to_gs.Reference_State.position_ref[0] = _DroneState.position[0];
                    Command_to_gs.Reference_State.position_ref[1] = _DroneState.position[1];
                    Command_to_gs.Reference_State.position_ref[2] = _DroneState.position[2];
                    Command_to_gs.Reference_State.velocity_ref[0] = 0;
                    Command_to_gs.Reference_State.velocity_ref[1] = 0;
                    Command_to_gs.Reference_State.velocity_ref[2] = 0;
                    Command_to_gs.Reference_State.acceleration_ref[0] = 0;
                    Command_to_gs.Reference_State.acceleration_ref[1] = 0;
                    Command_to_gs.Reference_State.acceleration_ref[2] = 0;
                    Command_to_gs.Reference_State.yaw_ref = _DroneState.attitude[2]; //rad
                }
                pos_sp[0] = Command_to_gs.Reference_State.position_ref[0];
                pos_sp[1] = Command_to_gs.Reference_State.position_ref[1];
                pos_sp[2] = Command_to_gs.Reference_State.position_ref[2];
                _command_to_mavros.send_pos_setpoint(pos_sp, Command_to_gs.Reference_State.yaw_ref);
                break;
            // 【Land】 降落。当前位置原地降落，降落后会自动上锁，
            case wrzf_pkg::ControlCommand::Land:
                Command_to_gs.Mode = Command_Now.Mode;
                Command_to_gs.Command_ID = Command_Now.Command_ID;
                _command_to_mavros.land();
                break;
            // 【Tracking_Cam】 跟随摄像头
            case wrzf_pkg::ControlCommand::Tracking_Cam:
                Command_to_gs.Mode = Command_Now.Mode;
                Command_to_gs.Command_ID = Command_Now.Command_ID;
                
                

                // Command_to_gs.Reference_State.position_ref[2] = _DroneState.position[2] + 0.01 * (camera_data.y_pos - CAM_CENTER_Z);
                
                // if (abs(camera_data.height_difference) <=4)
                // {
                //     Command_to_gs.Reference_State.yaw_ref = _DroneState.attitude[2];
                // }
                // else
                // {
                //     Command_to_gs.Reference_State.yaw_ref = _DroneState.attitude[2] - 0.001 * (camera_data.height_difference);
                // }
                
                // // Command_to_gs.Reference_State.yaw_ref = _DroneState.attitude[2] - 0.08 * (camera_data.height_difference / (camera_data.left_edge + camera_data.right_edge));
                
                
                // Command_to_gs.Reference_State.yaw_ref = yaw_ref_norm(Command_to_gs.Reference_State.yaw_ref);
                
                // thrust_sp = height_controller_cascade_pid.pos_controller(_DroneState, Command_to_gs.Reference_State, dt);

                // rpy_sp[0] = lr_controller_cascade_pid.pos_controller(CAM_CENTER_X - camera_data.x_pos, _DroneState, dt);

                // // rpy_sp[0] = + 0.001 * (camera_data.x_pos - CAM_CENTER_X);
                // rpy_sp[1] = forward_pitch;
                
                
                // if (camera_data.find_box_flag == false)
                // {
                // 	rpy_sp[0] = 0.0;
                // 	rpy_sp[1] = forward_pitch;
                // }
                // else
                // {
                // 	rpy_sp[2] = Command_to_gs.Reference_State.yaw_ref;
                // }
                
                // quaternion_sp = quaternion_from_rpy(rpy_sp);
                
                // _command_to_mavros.send_attitude_setpoint(quaternion_sp,thrust_sp);  






                d_pos_body[0] = 0;
                d_pos_body[1] = - 0.02 * (camera_data.x_pos - CAM_CENTER_X);

                px4_command_utils::rotation_yaw(_DroneState.attitude[2], d_pos_body, d_pos_enu);

                Command_to_gs.Reference_State.position_ref[0] = _DroneState.position[0] + d_pos_enu[0];
                Command_to_gs.Reference_State.position_ref[1] = _DroneState.position[1] + d_pos_enu[1];
                Command_to_gs.Reference_State.velocity_ref[0] = 0;
                Command_to_gs.Reference_State.velocity_ref[1] = 0;
                Command_to_gs.Reference_State.position_ref[2] = _DroneState.position[2] + 0.01 * (camera_data.y_pos - CAM_CENTER_Z);
                Command_to_gs.Reference_State.velocity_ref[2] = 0; 
                if (abs(camera_data.height_difference) <=0)
                {
                    Command_to_gs.Reference_State.yaw_ref = _DroneState.attitude[2];
                }
                else
                {
                    Command_to_gs.Reference_State.yaw_ref = _DroneState.attitude[2] - 0.002 * (camera_data.height_difference);
                }
/*
                if (radar_avoidance_data.avoidance_flag == true)
                {
                    switch (radar_avoidance_data.direction)
                    {
                        case TFLuna_i2c::RadarAvoidance::Up:
                            Command_to_gs.Reference_State.position_ref[2] = _DroneState.position[2] + 0.5;
                            break;
                        case TFLuna_i2c::RadarAvoidance::Down:
                            Command_to_gs.Reference_State.position_ref[2] = _DroneState.position[2] - 0.5;
                            break;
                        case TFLuna_i2c::RadarAvoidance::Left:
                            d_pos_body[0] = 0.0;
                            d_pos_body[1] = -0.5;
                            px4_command_utils::rotation_yaw(_DroneState.attitude[2], d_pos_body, d_pos_enu);
                            Command_to_gs.Reference_State.position_ref[0] = _DroneState.position[0] + d_pos_enu[0];
                            Command_to_gs.Reference_State.position_ref[1] = _DroneState.position[1] + d_pos_enu[1];
                            break;
                        case TFLuna_i2c::RadarAvoidance::Right:
                            d_pos_body[0] = 0.0;
                            d_pos_body[1] = 0.5;
                            px4_command_utils::rotation_yaw(_DroneState.attitude[2], d_pos_body, d_pos_enu);
                            Command_to_gs.Reference_State.position_ref[0] = _DroneState.position[0] + d_pos_enu[0];
                            Command_to_gs.Reference_State.position_ref[1] = _DroneState.position[1] + d_pos_enu[1];
                            break;
                        case TFLuna_i2c::RadarAvoidance::Hold:
                            
                            break;
                        case TFLuna_i2c::RadarAvoidance::Forward:
                            
                            break;
                        case TFLuna_i2c::RadarAvoidance::Back:
                           
                            break;
                    }
                }
                
                */
                // else
                // {
                //     pos_sp[0] = Command_to_gs.Reference_State.position_ref[0];
                //     pos_sp[1] = Command_to_gs.Reference_State.position_ref[1];
                //     pos_sp[2] = Command_to_gs.Reference_State.position_ref[2];

                //     _command_to_mavros.send_pos_setpoint(pos_sp, Command_to_gs.Reference_State.yaw_ref);
                // }

                pos_sp[0] = Command_to_gs.Reference_State.position_ref[0];
                pos_sp[1] = Command_to_gs.Reference_State.position_ref[1];
                pos_sp[2] = Command_to_gs.Reference_State.position_ref[2];

                _command_to_mavros.send_pos_setpoint(pos_sp, Command_to_gs.Reference_State.yaw_ref);
                
                break;
            // 【PathPatrol】 三次平滑轨迹跟随
            case wrzf_pkg::ControlCommand::PathPatrol:
                Command_to_gs.Mode = Command_Now.Mode;
                Command_to_gs.Command_ID = Command_Now.Command_ID;
                target_index = calc_target_index(_DroneState, tx, ty,ahead_distance,k);
                Command_to_gs.Reference_State.position_ref[0] = tx[target_index];
                Command_to_gs.Reference_State.position_ref[1] = ty[target_index];
                Command_to_gs.Reference_State.yaw_ref = tyaw[target_index];
                
                end_point.x = x.back();
                end_point.y = y.back();
                end_point.z = Command_to_gs.Reference_State.position_ref[2];

                if(target_index == tx.size() - 1)
                {
                    waypoint = waypoint_trajectory(_DroneState, end_point);
                    Command_to_gs.Reference_State.position_ref[0] = waypoint.x;
                    Command_to_gs.Reference_State.position_ref[1] = waypoint.y;
                }
                

                pos_sp[0] = Command_to_gs.Reference_State.position_ref[0];
                pos_sp[1] = Command_to_gs.Reference_State.position_ref[1];
                pos_sp[2] = Command_to_gs.Reference_State.position_ref[2];
                _command_to_mavros.send_pos_setpoint(pos_sp, Command_to_gs.Reference_State.yaw_ref);
                break;
            // Circle_Tracking 轨迹追踪控制，与上述追踪点或者追踪速度不同，此时期望输入为一段圆轨迹
            case wrzf_pkg::ControlCommand::Circle_Tracking:
                Command_to_gs.Mode = Command_Now.Mode;
                Command_to_gs.Command_ID = Command_Now.Command_ID;
                
                if (Command_Last.Mode != wrzf_pkg::ControlCommand::Circle_Tracking)
                {
                    time_trajectory = 0.0;
                }

                time_trajectory = time_trajectory + dt;

                Command_to_gs.Reference_State = _Circle_Trajectory.Circle_trajectory_generation(time_trajectory);

                pos_sp[0] = Command_to_gs.Reference_State.position_ref[0];
                pos_sp[1] = Command_to_gs.Reference_State.position_ref[1];
                pos_sp[2] = Command_to_gs.Reference_State.position_ref[2];

                _command_to_mavros.send_pos_setpoint(pos_sp, Command_to_gs.Reference_State.yaw_ref);
                // Quit  悬停于最后一个目标点
                if (time_trajectory >= _Circle_Trajectory.time_total)
                {
                    Command_Now.Mode = wrzf_pkg::ControlCommand::Move_ENU;
                    Command_Now.Reference_State = Command_to_gs.Reference_State;
                }
                break;
            // Eight_Tracking 轨迹追踪控制，与上述追踪点或者追踪速度不同，此时期望输入为一段8字轨迹
            case wrzf_pkg::ControlCommand::Eight_Tracking:
                Command_to_gs.Mode = Command_Now.Mode;
                Command_to_gs.Command_ID = Command_Now.Command_ID;
                
                if (Command_Last.Mode != wrzf_pkg::ControlCommand::Eight_Tracking)
                {
                    time_trajectory = 0.0;
                }

                time_trajectory = time_trajectory + dt;

                Command_to_gs.Reference_State = _Eight_Trajectory.Eight_trajectory_generation(time_trajectory);

                pos_sp[0] = Command_to_gs.Reference_State.position_ref[0];
                pos_sp[1] = Command_to_gs.Reference_State.position_ref[1];
                pos_sp[2] = Command_to_gs.Reference_State.position_ref[2];

                _command_to_mavros.send_pos_setpoint(pos_sp, Command_to_gs.Reference_State.yaw_ref);
                // Quit  悬停于最后一个目标点
                if (time_trajectory >= _Eight_Trajectory.time_total)
                {
                    Command_Now.Mode = wrzf_pkg::ControlCommand::Move_ENU;
                    Command_Now.Reference_State = Command_to_gs.Reference_State;
                }
                break;
        }
        
        
        if(Flag_printf == 1)
        {
            //打印无人机状态
            px4_command_utils::prinft_drone_state(_DroneState);
            // 打印上层控制指令
            px4_command_utils::printf_command_control(Command_to_gs);

        }else if(((int)(cur_time*10) % 50) == 0)
        {
            cout << "px4_pos_controller is running for :" << cur_time << " [s] "<<endl;
        }
        _Topic_for_log.header.stamp = ros::Time::now();
        _Topic_for_log.Drone_State = _DroneState;
        _Topic_for_log.Control_Command = Command_to_gs;
        // _Topic_for_log.Attitude_Reference = _AttitudeReference;
        // _Topic_for_log.Control_Output = _ControlOutput;

        log_pub.publish(_Topic_for_log);

        Command_Last = Command_Now;

        rate.sleep();
    }

    return 0;
}

void printf_param()
{
    cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>> Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    cout << "Takeoff_height: "<< Takeoff_height<<" [m] "<<endl;
    cout << "Disarm_height : "<< Disarm_height <<" [m] "<<endl;
    cout << "geo_fence_x : "<< geo_fence_x[0] << " [m]  to  "<<geo_fence_x[1] << " [m]"<< endl;
    cout << "geo_fence_y : "<< geo_fence_y[0] << " [m]  to  "<<geo_fence_y[1] << " [m]"<< endl;
    cout << "geo_fence_z : "<< geo_fence_z[0] << " [m]  to  "<<geo_fence_z[1] << " [m]"<< endl;
    
}

int check_failsafe()
{
    if (_DroneState.position[0] < geo_fence_x[0] || _DroneState.position[0] > geo_fence_x[1] ||
        _DroneState.position[1] < geo_fence_y[0] || _DroneState.position[1] > geo_fence_y[1] ||
        _DroneState.position[2] < geo_fence_z[0] || _DroneState.position[2] > geo_fence_z[1])
    {
        return 1;
        cout << "Out of the geo fence, the drone is landing... "<< endl;
    }
    else{
        return 0;
    }
}
